#!/usr/bin/env python3

import rospy
import tf
import csv
from geometry_msgs.msg import PoseStamped

class PathPublisherNode:
    def __init__(self):
        # 初始化节点
        rospy.init_node('path_publisher_node')
        
        # 获取参数
        self.file_path = rospy.get_param('~file_path')
        self.threshold = 1.5
        self.point_index = 0
        
        # 读取CSV文件
        self.path = self._read_csv()
        
        # 创建TF监听器
        self.tf_listener = tf.TransformListener()
        
        # 创建发布者（修改为PoseStamped类型）
        self.pub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=10)
        
        # 创建0.5Hz定时器
        self.timer = rospy.Timer(rospy.Duration(2.0), self.timer_callback)
        
        rospy.sleep(5.0)
        rospy.loginfo("Path publisher node就绪")

    def _read_csv(self):
        """读取CSV文件中的路径点"""
        path = []
        with open(self.file_path, 'r') as f:
            reader = csv.DictReader(f)
            for row in reader:
                path.append({
                    'x': float(row['x']),
                    'y': float(row['y']),
                    'yaw': float(row['z']) 
                })
        return path

    def timer_callback(self, event):
        """定时器回调函数"""
        try:
            # 获取map到body的TF变换
            (trans, rot) = self.tf_listener.lookupTransform('/map', '/body', rospy.Time(0))
            
            # 计算当前姿态
            current_x, current_y = trans[0], trans[1]
            euler = tf.transformations.euler_from_quaternion(rot)
            current_yaw = euler[2]
            
            # 获取目标点
            target = self.path[self.point_index]
            target_x, target_y, target_yaw = target['x'], target['y'], target['yaw']
            
            # 计算偏差总和（修正为与目标的差值）
            yaw_diff = abs(current_yaw - target_yaw)
            deviation = abs(current_x - target_x) + abs(current_y - target_y) + yaw_diff
            
            # 判断是否切换到下一个点
            if deviation < self.threshold:
                self.point_index = (self.point_index + 1) % len(self.path)
                rospy.loginfo(f"Switching to point {self.point_index}")
            
            # 发布当前点（修改为发布Pose）
            self.publish_pose(target)
            
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
            rospy.logwarn(f"TF异常: {e}")

    def publish_pose(self, point):
        """发布目标姿态"""
        msg = PoseStamped()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "map"
        
        # 设置位置
        msg.pose.position.x = point['x']
        msg.pose.position.y = point['y']
        msg.pose.position.z = 0.0  # 2D导航默认z=0
        
        # 设置姿态（从欧拉角转换为四元数）
        quat = tf.transformations.quaternion_from_euler(0, 0, point['yaw'])
        msg.pose.orientation.x = quat[0]
        msg.pose.orientation.y = quat[1]
        msg.pose.orientation.z = quat[2]
        msg.pose.orientation.w = quat[3]
        
        self.pub.publish(msg)

if __name__ == '__main__':
    try:
        node = PathPublisherNode()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass